#include <rtthread.h>
#include <rtdevice.h>
#include "icm20608.h"
#include "icm20608_example.h"

#define I2C_BUS_NAME   "i2c2"   // I2C 总线名称
#define ICM20608_DEVICE_NAME  "icm20608" // ICM20608 设备名
#define THREAD_STACK_SIZE    1024  // 线程栈大小
#define THREAD_PRIORITY      10    // 线程优先级
#define THREAD_TIMESLICE     5     // 线程时间片

static icm20608_device_t icm20608_dev = RT_NULL;  // ICM20608 设备句柄

// 定义外部变量
float accel_x_g = 0.0f, accel_y_g = 0.0f, accel_z_g = 0.0f;
float gyro_x_dps = 0.0f, gyro_y_dps = 0.0f, gyro_z_dps = 0.0f;

// 转换加速度计原始数据为实际加速度（单位：g）
static float accel_raw_to_g(rt_int16_t raw_value, float range)
{
    return ((float)raw_value / 32768) * range;
}

// 转换陀螺仪原始数据为实际角速度（单位：°/s）
static float gyro_raw_to_deg_per_sec(rt_int16_t raw_value, float range)
{
    return ((float)raw_value / 32768) * range;
}

// 读取加速度计和陀螺仪数据并保存到外部变量中
static void icm20608_read_data(void)
{
    rt_int16_t accel_x, accel_y, accel_z;
    rt_int16_t gyro_x, gyro_y, gyro_z;

    // 获取加速度计数据
    if (icm20608_get_accel(icm20608_dev, &accel_x, &accel_y, &accel_z) == RT_EOK)
    {
        // 假设加速度计量程是 ±8g
        accel_x_g = accel_raw_to_g(accel_x, 8.0f);
        accel_y_g = accel_raw_to_g(accel_y, 8.0f);
        accel_z_g = accel_raw_to_g(accel_z, 8.0f);

        // rt_kprintf("Accel - X: %.4f g, Y: %.4f g, Z: %.4f g\n", accel_x_g, accel_y_g, accel_z_g);
    }
    else
    {
        rt_kprintf("Failed to get accelerometer data\n");
    }

    // 获取陀螺仪数据
    if (icm20608_get_gyro(icm20608_dev, &gyro_x, &gyro_y, &gyro_z) == RT_EOK)
    {
        // 假设陀螺仪量程是 ±1000°/s
        gyro_x_dps = gyro_raw_to_deg_per_sec(gyro_x, 1000.0f);
        gyro_y_dps = gyro_raw_to_deg_per_sec(gyro_y, 1000.0f);
        gyro_z_dps = gyro_raw_to_deg_per_sec(gyro_z, 1000.0f);

        // rt_kprintf("Gyro - X: %.4f °/s, Y: %.4f °/s, Z: %.4f °/s\n", gyro_x_dps, gyro_y_dps, gyro_z_dps);
    }
    else
    {
        rt_kprintf("Failed to get gyroscope data\n");
    }
}

// 线程入口函数
static void icm20608_thread_entry(void *parameter)
{
    rt_err_t result;

    // 初始化 ICM20608 设备
    icm20608_dev = icm20608_init(I2C_BUS_NAME);
    if (icm20608_dev == RT_NULL)
    {
        rt_kprintf("ICM20608 initialization failed!\n");
        return;
    }
    rt_kprintf("ICM20608 initialized successfully!\n");

    // 设置加速度计和陀螺仪的量程（可根据需要调整）
    result = icm20608_set_param(icm20608_dev, ICM20608_ACCEL_CONFIG1, ICM20608_ACCELEROMETER_RANGE2); // ±8g
    if (result != RT_EOK)
    {
        rt_kprintf("Failed to set accelerometer range\n");
    }
    result = icm20608_set_param(icm20608_dev, ICM20608_GYRO_CONFIG, ICM20608_GYROSCOPE_RANGE2); // ±1000°/s
    if (result != RT_EOK)
    {
        rt_kprintf("Failed to set gyroscope range\n");
    }

    // 循环读取数据
    while (1)
    {
        icm20608_read_data();  // 读取并打印加速度计和陀螺仪数据
        rt_thread_mdelay(2000);  // 每隔 2000ms 读取一次数据
    }

    // 在程序结束时释放资源
    icm20608_deinit(icm20608_dev);
    rt_kprintf("ICM20608 deinitialized!\n");
}

// 创建线程
int icm20608_thread_init(void)
{
    rt_thread_t thread;

    // 创建读取 ICM20608 数据的线程
    thread = rt_thread_create("icm20608", icm20608_thread_entry, RT_NULL,
                              THREAD_STACK_SIZE, THREAD_PRIORITY, THREAD_TIMESLICE);

    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);  // 启动线程
        return RT_EOK;
    }
    else
    {
        rt_kprintf("Failed to create ICM20608 thread\n");
        return -RT_ERROR;
    }
}
